
#include <stdio.h>
#include "member_BConearRestraint.h"
#include "member_BRestraint.h"
#include "member_BBody.h"
#include "member_BNode.h"

using namespace BWE;

member_BConearRestraint::member_BConearRestraint(BConearRestraint* restraint)
{
	boss = restraint;

	axisA.set(1, 0, 0);
	axisB.set(1, 0, 0);

	solveTwistLimit = false;
	solveSwingLimit = false;
	motorEnabled = false;
	maxMotorImpulse = BReal(-1);

	swingSpan1 = BReal(0.3);
	swingSpan2 = BReal(0.8);

	twistSpan = 0;// REAL_LARGE;

	biasFactor = 1;
	damping = BReal(0.1);
}
member_BConearRestraint::~member_BConearRestraint()
{

}

void member_BConearRestraint::calculateAngleInfo()
{
	swingError = 0;
	twistLimitSign = 0;
	solveTwistLimit = false;
	solveSwingLimit = false;

	const BBody* bodyA = restraint_member(boss)->bodyA;
	const BBody* bodyB = restraint_member(boss)->bodyB;

	const BMatrix& matrixA = node_member(bodyA)->matrix;
	const BMatrix& matrixB = node_member(bodyB)->matrix;

	const BMatrix& invInertiaA = body_member(bodyA)->invInertia;
	const BMatrix& invInertiaB = body_member(bodyB)->invInertia;

	frameA.setRotate(BQuater(BVector(1, 0, 0), axisA));
	frameB.setRotate(BQuater(BVector(1, 0, 0), axisB));

	if (motorEnabled)
	{
		BMatrix targetPose(target);
		BMatrix transformA = matrixA * frameA;
		BMatrix transformB = matrixB * frameB;
		BMatrix deltaAB = transformB * targetPose * transformA.inverse();
		BQuater deltaRotate = deltaAB.rotate();
		swingAxis = deltaRotate.axis();
		if (bNonZero(swingAxis.length2()))
		{
			swingError = deltaRotate.angle();
			if (bNonZero(swingError))
			{
				solveSwingLimit = true;
			}
		}
	}

	BVector axis0 = matrixA.mult3(frameA.row(0));
	BVector axis1 = matrixA.mult3(frameA.row(1));
	BVector axis2 = matrixA.mult3(frameA.row(2));
	BVector axisB0 = matrixB.mult3(frameB.row(0));
	BReal x = axisB0.dot(axis0);
	BReal y = axisB0.dot(axis1);
	BReal z = axisB0.dot(axis2);

	if (swingSpan1 < REAL_EPSILON && swingSpan2 < REAL_EPSILON)
	{
		if (!bZero(y) || !bZero(z))
		{
			solveSwingLimit = true;
			swingAxis = -axisB0.cross(axis0);
		}
	}
	else if (swingSpan1 > REAL_EPSILON && swingSpan2 > REAL_EPSILON)
	{
		BMatrix transFrameA = matrixA * frameA;
		BMatrix transFrameB = matrixB * frameB;
		BMatrix matrixAB = transFrameB.inverse() * transFrameA;
		coneAxis = matrixA.mult3(axisA);
		coneAxis.normalize();
		horAxis = axis1;
		verAxis = axis2;
		BVector realAxisB = matrixB.mult3(axisB);
		BQuater coneQuater(realAxisB, coneAxis);
		coneQuater.normalize();
		
		BReal swingLimit = swingSpan1;
		BReal swingAngle = coneQuater.angle();
		if (swingAngle > REAL_EPSILON)
		{
			swingAxis = BVector(coneQuater.x(), coneQuater.y(), coneQuater.z());
			swingAxis.normalize();
			BReal xEllipse = swingAxis.dot(horAxis);
			BReal yEllipse = swingAxis.dot(verAxis);
			if (fabs(xEllipse) > REAL_EPSILON)
			{
				BReal slope2 = (yEllipse * yEllipse) / (xEllipse * xEllipse);
				BReal norm = 1 / (swingSpan2 * swingSpan2) + slope2 / (swingSpan1 * swingSpan1);
				BReal swingLimit2 = (1 + slope2) / norm;
				swingLimit = sqrt(swingLimit2);
			}
		}

		if (swingAngle > swingLimit)
		{
			solveSwingLimit = true;
			swingLimitRatio = 1;
			swingError = swingAngle - (swingLimit);
			swingAxis = -swingAxis;
		}
	}
	else
	{
		if (swingSpan1 > REAL_EPSILON)
		{
			if (!bZero(x) || !bZero(y))
			{
				solveSwingLimit = true;
				z = 0;
				BReal span1 = atan2(y, x);
				if (span1 > swingSpan1)
				{
					x = cos(swingSpan1);
					y = sin(swingSpan1);
				}
				else if (span1 < -swingSpan1)
				{
					x = cos(swingSpan1);
					y = -sin(swingSpan1);
				}
			}
		}
		if (swingSpan2 > REAL_EPSILON)
		{
			if (!bZero(x) || !bZero(z))
			{
				solveSwingLimit = true;
				y = 0;
				BReal span2 = atan2(z, x);
				if (span2 > swingSpan2)
				{
					x = cos(swingSpan2);
					z = sin(swingSpan2);
				}
				else if (span2 < -swingSpan2)
				{
					x = cos(swingSpan2);
					z = -sin(swingSpan2);
				}
			}
		}
		BVector target;
		target.x() = x * axis0.x() + y * axis1.x() + z * axis2.x();
		target.y() = x * axis0.y() + y * axis1.y() + z * axis2.y();
		target.z() = x * axis0.z() + y * axis1.z() + z * axis2.z();
		target.normalize();
		swingAxis = -axisB0.cross(target);
		swingError = swingAxis.length();
		if (!bZero(swingError))
			swingAxis.normalize();
	}

	if (twistSpan > 0)
	{
		BQuater qA = matrixA.rotate() * frameA.rotate();
		BQuater qB = matrixB.rotate() * frameB.rotate();
		BQuater qAB = qB.inverse() * qA;
		BVector vConeNoTwist = qAB * BVector(1, 0, 0);
		vConeNoTwist.normalize();
		BQuater qABCone(BVector(1, 0, 0), vConeNoTwist);
		qABCone.normalize();
		BQuater qABTwist = qABCone.inverse() * qAB;
		qABTwist.normalize();

		BQuater minTwist = qABTwist;
		twistAngle = qABTwist.angle();
		if (twistAngle > PI)
		{
			minTwist = -qABTwist;
			twistAngle = minTwist.angle();
		}
		twistAxis.set(minTwist.x(), minTwist.y(), minTwist.z());
		if (twistAngle > REAL_EPSILON)
			twistAxis.normalize();
	
		if (twistAngle > twistSpan)
		{
			solveTwistLimit = true;
			twistLimitRatio = 1;
			twistError = twistAngle - twistSpan;
			twistAxis = qB * -twistAxis;
		}
	}
	else
	{
		twistAngle = 0;
	}
}
BReal member_BConearRestraint::atan2Fast(BReal y, BReal x)
{
	BReal coeff_1 = PI / 4;
	BReal coeff_2 = 3 * coeff_1;
	BReal abs_y = (BReal)fabs(y);
	BReal angle;
	if (x >= 0)
	{
		BReal r = (x - abs_y) / (x + abs_y);
		angle = coeff_1 - coeff_1 * r;
	}
	else
	{
		BReal r = (x + abs_y) / (abs_y - x);
		angle = coeff_2 - coeff_1 * r;
	}
	return (y < 0) ? -angle : angle;
}
